#include "./ctrl.h"
#include "../xlib/gpio.h"
#include "../rtx/rtx_os.h"
#include "./sensor.h"

namespace main_driver {

Ctrl ctrl;

namespace ctrl_impl {

using namespace stm32f1;
using namespace gpio;

using PICK1_LED = PB<0>;
using PICK2_LED = PB<9>;
using STRAW_LED = PB<10>;
using LIGHT_LED = PB<11>;
using DROP1 = PB<12>;
using DROP2 = PB<13>;
using ROBOT_RESET = PB<14>;
using LOCK = PB<15>;

}

using namespace ctrl_impl;

void Ctrl::init() {

    rcc::enable<
        PICK1_LED, PICK2_LED, STRAW_LED,
        LIGHT_LED, DROP1, DROP2,
        ROBOT_RESET, LOCK
    >();

    config<
        Cfg<PICK1_LED, Mode::OUT_50MHz_PP>,
        Cfg<PICK2_LED, Mode::OUT_50MHz_PP>,
        Cfg<STRAW_LED, Mode::OUT_50MHz_PP>,
        Cfg<LIGHT_LED, Mode::OUT_50MHz_PP>,
        Cfg<DROP1, Mode::OUT_50MHz_PP>,
        Cfg<DROP2, Mode::OUT_50MHz_PP>,
        Cfg<ROBOT_RESET, Mode::OUT_50MHz_PP>,
        Cfg<LOCK, Mode::OUT_50MHz_PP>
    >();

    PICK1_LED::clr();
    PICK2_LED::clr();
    STRAW_LED::clr();
    LIGHT_LED::clr();
    DROP1::clr();
    DROP2::clr();
    ROBOT_RESET::clr();
    LOCK::clr();

    
}

void Ctrl::open_led(Parts parts) {

    switch (parts) {
        case Parts::PICK1_LED: PICK1_LED::set(); break;
        case Parts::PICK2_LED: PICK2_LED::set(); break;
        case Parts::STRAW_LED: STRAW_LED::set(); break;
        case Parts::LIGHT_LED: LIGHT_LED::set(); break;
    }
}

void Ctrl::close_led(Parts parts) {
    switch (parts) {
        case Parts::PICK1_LED: PICK1_LED::clr(); break;
        case Parts::PICK2_LED: PICK2_LED::clr(); break;
        case Parts::STRAW_LED: STRAW_LED::clr(); break;
        case Parts::LIGHT_LED: LIGHT_LED::clr(); break;
    }
}

Error Ctrl::drop1() {
    Error err;
    uint32_t time_count = 0;

    DROP1::set();
    for (;;) {
        if (!sensor.is_drop_cup1_checked()) {
            break;
        }
        osDelay(50);
        time_count += 50;
        if (time_count >= DROP_TIMEOUT) {
            err = ec::main::DROP1_TIMEOUT;
            goto finish;
        }
    }

    time_count = 0;
    for (;;) {
        if (sensor.is_drop_cup1_checked()) {
            break;
        }
        osDelay(50);
        time_count += 50;
        if (time_count >= DROP_TIMEOUT) {
            err = ec::main::DROP1_TIMEOUT;
            goto finish;
        }
    }

finish:
    DROP1::clr();
    return err;
}

Error Ctrl::drop2() {
    Error err;
    uint32_t time_count = 0;

    DROP2::set();
    for (;;) {
        if (!sensor.is_drop_cup2_checked()) {
            break;
        }
        osDelay(50);
        time_count += 50;
        if (time_count >= DROP_TIMEOUT) {
            err = ec::main::DROP2_TIMEOUT;
            goto finish;
        }
    }

    time_count = 0;
    for (;;) {
        if (sensor.is_drop_cup2_checked()) {
            break;
        }
        osDelay(50);
        time_count += 50;
        if (time_count >= DROP_TIMEOUT) {
            err = ec::main::DROP2_TIMEOUT;
            goto finish;
        }
    }

finish:
    DROP2::clr();
    return err;
}

void Ctrl::lock_open() {
    LOCK::set();
    osDelay(500);
    LOCK::clr();
}

void Ctrl::robot_reset() {
    ROBOT_RESET::set();
    osDelay(500);
    ROBOT_RESET::clr();
}

}
